import time
import threading
import serial  # pyserial
import sys
import os

# ==================== 配置区 ====================
# 【重要】修改为你的 BrainLink 串口
# BRAIN_PORT = "/dev/cu.BrainLink_Pro"   # macOS 示例
BRAIN_PORT = "COM5"                  # Windows 示例（改成你的实际 COM 口）

# 控制阈值（0-100），可自行调整
ATTENTION_THRESHOLD = 80   # 注意力超过此值 → 起飞
MEDITATION_THRESHOLD = 80  # 冥想超过此值 → 降落

DEBOUNCE_SEC = 4           # 防误触冷却时间（秒）
# ===============================================

# 全局变量
latest_data = None
data_lock = threading.Lock()
is_flying = False
last_action_time = 0

# BrainLink 回调函数
def on_eeg(data):
    """主 EEG 数据回调：注意力、冥想等"""
    global latest_data
    with data_lock:
        latest_data = data
    print(f"【实时脑波】注意力: {data.attention:3d} | 冥想: {data.meditation:3d} | "
          f"delta: {data.delta:4d} theta: {data.theta:4d}")

def on_extend_eeg(data):
    """扩展数据（电量、心率等）"""
    print(f"【扩展信息】电量: {data.battery}%  温度: {data.temperature}°C")

def on_gyro(x, y, z):
    pass  # 陀螺仪数据（可后续扩展控制方向）

def on_rr(rr1, rr2, rr3):
    pass

def on_raw(raw):
    pass

# 串口读取线程（持续读取 BrainLink 数据）
def serial_reading_thread(port):
    global parser
    try:
        ser = serial.Serial(port, 115200, timeout=1)
        print(f"✅ BrainLink 串口已打开: {port}")
        
        # 初始化解析器（必须按顺序传 5 个回调）
        parser = BrainLinkParser(on_eeg, on_extend_eeg, on_gyro, on_rr, on_raw)
        
        while True:
            if ser.in_waiting > 0:
                byte_data = ser.read(ser.in_waiting)
                if byte_data:
                    parser.parse(byte_data)
            time.sleep(0.01)
    except Exception as e:
        print(f"❌ 串口错误: {e}")
        sys.exit(1)

# 主程序
if __name__ == "__main__":
    # 确保 BrainLinkParser 在当前目录可被 import
    sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
    
    # 启动 BrainLink 读取线程
    thread = threading.Thread(target=serial_reading_thread, args=(BRAIN_PORT,), daemon=True)
    thread.start()
    
    # 初始化 Tello
    tello = Tello()
    tello.connect()
    print(f"✅ Tello 已连接，当前电量: {tello.get_battery()}%")
    
    print("\n🚀 系统已就绪！")
    print(f"   注意力 > {ATTENTION_THRESHOLD} → 自动起飞")
    print(f"   冥想 > {MEDITATION_THRESHOLD} → 自动降落")
    print("   Ctrl+C 安全退出\n")
    
    try:
        while True:
            with data_lock:
                if latest_data is None:
                    time.sleep(0.3)
                    continue
                
                att = latest_data.attention
                med = latest_data.meditation
                now = time.time()
                
                # 防抖 + 执行命令
                if now - last_action_time > DEBOUNCE_SEC:
                    if att >= ATTENTION_THRESHOLD and not is_flying:
                        print("🧠 高注意力检测 → 执行【起飞】")
                        try:
                            tello.takeoff()
                            is_flying = True
                            last_action_time = now
                        except Exception as e:
                            print(f"起飞失败: {e}")
                    
                    elif med >= MEDITATION_THRESHOLD and is_flying:
                        print("🧘 高冥想检测 → 执行【降落】")
                        try:
                            tello.land()
                            is_flying = False
                            last_action_time = now
                        except Exception as e:
                            print(f"降落失败: {e}")
            
            time.sleep(0.2)  # 主循环频率
    
    except KeyboardInterrupt:
        print("\n⛔ 用户中断，安全降落...")
        if is_flying:
            try:
                tello.land()
            except:
                pass
        print("程序已安全退出！")